import serial
import struct
import numpy as np
from scipy.signal import butter, filtfilt, freqz
import matplotlib.pyplot as plt
import time
import threading
import subprocess
import os

exit_event = threading.Event()

def exit_listener():
    """后台监听回车键的线程"""
    input()  # 阻塞直到用户按下回车
    print("\n检测到回车键，程序已终止...")
    exit_event.set()


# 配置串口参数
port = 'COM3'  # 根据实际情况修改串口
baudrate = 115200  # 根据实际情况修改波特率

# 打开串口
ser = serial.Serial(port, baudrate, timeout=1)


# 数据帧结构解析函数
def parse_frame(frame):
    # 帧头校验
    """
    if frame[:2] != b'\xaa\xaa':
        print("无效的帧头")
        return None

    # 提取长度帧
    length = frame[2]

    # 校验帧尾
    if frame[-1] != 0x55:
        print("无效的帧尾")
        return None

    # 校验长度帧是否匹配（长度帧不包含帧头和帧尾）
    expected_length = len(frame) - 3  # 减去帧头(2字节)和长度帧本身(1字节)
    if length != expected_length:
        print(f"长度帧不匹配，期望长度: {expected_length}, 实际长度: {length}")
        print(frame)
        return None
    """
    # 解析时间戳（大端4字节）
    timestamp = struct.unpack('>I', frame[3:7])[0]

    # 解析加速度（3字节）
    acc_bytes = frame[7:10]
    acc_x = struct.unpack('>h', acc_bytes[:2])[0] / 32768.0  # 假设是16位有符号整数，除以最大值进行归一化
    acc_y = struct.unpack('>h', acc_bytes[1:3])[0] / 32768.0
    # 由于只有3字节，第三个字节单独处理
    acc_z = struct.unpack('>b', acc_bytes[2:3])[0] / 128.0

    # 解析陀螺仪（3字节），同理
    gyro_bytes = frame[10:13]
    gyro_x = struct.unpack('>h', gyro_bytes[:2])[0] / 32768.0
    gyro_y = struct.unpack('>h', gyro_bytes[1:3])[0] / 32768.0
    gyro_z = struct.unpack('>b', gyro_bytes[2:3])[0] / 128.0

    # 解析角度（3字节），同理
    angle_bytes = frame[13:16]
    pitch = struct.unpack('>h', angle_bytes[:2])[0] / 32768.0
    roll = struct.unpack('>h', angle_bytes[1:3])[0] / 32768.0
    yaw = struct.unpack('>b', angle_bytes[2:3])[0] / 128.0

    # 解析EMG数据（每个EMG8字节）
    emg_data = {}
    emg_start_index = 16
    for i in range(1, 11):  # EMG1到EMG10
        emg_bytes = frame[emg_start_index:emg_start_index + 8]
        emg_values = struct.unpack('8B', emg_bytes)  # 假设每个通道是8位无符号整数
        emg_data[f'EMG{i}'] = emg_values
        emg_start_index += 8

    # 解析电池电量（1字节）
    battery_byte = frame[emg_start_index]
    battery_percentage = battery_byte / 0x64 * 100  # 0x64对应100%

    return {
        'timestamp': timestamp,
        'accelerometer': {'x': acc_x, 'y': acc_y, 'z': acc_z},
        'gyroscope': {'x': gyro_x, 'y': gyro_y, 'z': gyro_z},
        'angle': {'pitch': pitch, 'roll': roll, 'yaw': yaw},
        'emg': emg_data,
        'battery': battery_percentage
    }

threading.Thread(target=exit_listener, daemon=True).start()
# 主循环读取串口数据
try:
    file_name_1 = "acc_data.txt"
    file_name_2 = "emg_data.txt"

    # 检查文件是否存在
    if os.path.exists(file_name_1):
        try:
            # 删除文件
            os.remove(file_name_1)
            print(f"文件 {file_name_1} 已成功删除")
        except Exception as e:
            print(f"删除文件时发生错误: {str(e)}")
    else:
        print(f"文件 {file_name_1} 不存在于当前目录")

    if os.path.exists(file_name_2):
        try:
            # 删除文件
            os.remove(file_name_2)
            print(f"文件 {file_name_2} 已成功删除")
        except Exception as e:
            print(f"删除文件时发生错误: {str(e)}")
    else:
        print(f"文件 {file_name_2} 不存在于当前目录")

    buffer = b''  # 用于拼接不完整的数据帧
    exe_path = r"WindowsApp2（离散动作）\WindowsApp2\bin\Debug\WindowsApp2.exe"
    subprocess.Popen(exe_path, shell=True)
    time1 = time.time()
    while not exit_event.is_set():
        # 读取数据
        data = ser.read(ser.in_waiting or 1)  # 至少读取1字节，或全部可用数据
        # print(data)
        buffer += data
        # 查找帧尾0x55来分割帧
        while True:
            frame_end_index = buffer.find(b'\x55')
            if frame_end_index == -1:
                break  # 没有找到帧尾，等待更多数据

            # 检查帧头是否完整
            if len(buffer) < frame_end_index + 1 or buffer[:2] != b'\xaa\xaa':
                buffer = buffer[1:]  # 丢弃第一个字节，继续查找
                continue
            if len(buffer) < 98:
                break
            # 提取完整帧
            frame = buffer[:98]
            buffer = buffer[98:]
            # print(len(frame))

            # 尝试解析帧
            parsed_data = parse_frame(frame)
            # print(parsed_data)
            if parsed_data:
                acc = parsed_data['angle']
                accx = np.array(acc['pitch']) * 180
                accy = np.array(acc['roll']) * 180
                accz = np.array(acc['yaw']) * 180
                acc_data = np.column_stack((accx, accy, accz))
                with open("acc_data.txt", "a") as f:
                    np.savetxt(f, acc_data, fmt="%.2f", delimiter=",")
                emg = parsed_data['emg']
                # print(EMG_data.shape)
                for i in range(1, 11):
                    emg_data = (np.array(emg[f'EMG{i}']) / 255 * 3.3 - 1.65) / 200 * 1000000
                    emg_data = emg_data.reshape(1, -1)  # -1 表示自动计算列数
                    with open("emg_data.txt", "a") as f:
                        np.savetxt(f, emg_data, fmt="%.2f", delimiter=",")  # 保持与原始文件一致的格式
                    # f.write("\n")  # 添加两个换行符（创建空行）
except KeyboardInterrupt:
    time2 = time.time()
    print("用户中断")
    print(time2 - time1)
finally:
    time2 = time.time()
    print(time2-time1)
    ser.close()